/*
 * 文件名：endstop.cpp
 * 功能描述：限位开关校准和位置检测
 */

#include "endstop.h"
#include "config.h"
#include "serial_output.h"
#include "power_management.h"
// #include "scara_motion.h"
// 定义标志变量
bool Xcalibrated = false;
bool Ycalibrated = false;

void endstop_init()
{
    pinMode(endstop_X, INPUT_PULLUP); // 引脚 2 设置为上拉输入（X轴限位开关）
    pinMode(endstop_Y, INPUT_PULLUP); // 引脚 14 设置为上拉输入（Y轴限位开关）

    // 有时候CB会出现无法归位的情况，这个是判断其引脚是否损坏
    //    pinMode(endstop_X, INPUT);  // 引脚 2 设置为上拉输入（X轴限位开关）
    //    pinMode(endstop_Y, INPUT); // 引脚 14 设置为上拉输入（Y轴限位开关）
    //    Serial.println(analogRead(endstop_X));
    // Serial.println(analogRead(endstop_Y));
}

/**
 * @brief 校准X轴限位开关
 * @note 包含50秒超时保护机制
 *
 * 校准流程：
 * 1. 持续移动XY两个电机直至触发限位
 * 2. 重置电机位置计数器
 * 3. 返回安全位置并更新校准状态
 */
void calibrateX()
{
    unsigned long startTime = millis(); // 记录开始时间
    unsigned long timeout = 50000;      // 设置超时时间为10秒
    while (digitalRead(endstop_X) != LOW)
    {

        // 向限位开关方向移动直到触发
        Xaxis.move(10000);  // 每次移动500步
        Xaxis.run();        // 执行运动
        Yaxis.move(-10000); // 每次移动500步
        Yaxis.run();        // 执行运动
        // 检查X轴校准是否超时
        if (millis() - startTime > timeout)
        {
            if (print_key)
            {
                Serial.println("X轴校准超时");
            }
            return; // 超时，退出函数
        }
    }

    // X轴限位开关触发，停止X轴电机并设置当前位置为0
    Xaxis.setCurrentPosition(0); // 设置X轴当前位置为0
    Yaxis.setCurrentPosition(0); // 设置X轴当前位置为0

    Xaxis.moveTo(X_RESET_STEPS); // 设置X轴目标位置为850步
    while (Xaxis.distanceToGo() != 0)
    {
        Xaxis.run(); // 持续执行直到电机到达目标位置
    }
    Xaxis.stop();       // 当电机到达目标位置后停止电机
    Xcalibrated = true; // 标记X轴已校准
}
// 有时候限位开关坏了之后换引脚重新连接，焊接不好干扰大会提前归位，这个加了一个滤波
//  X轴校准函数
//  void calibrateX() {
//      unsigned long startTime = millis(); // 记录开始时间
//      unsigned long timeout = 50000; // 设置超时时间为10秒
//      int stableCount = 0;
//      const int STABILITY_THRESHOLD = 1;

//     while (stableCount < STABILITY_THRESHOLD) {
//         // 向限位开关方向移动直到触发
//         Xaxis.move(10000);  // 每次移动500步
//         Xaxis.run();       // 执行运动
//         Yaxis.move(-10000);  // 每次移动500步
//         Yaxis.run();       // 执行运动

//         if (digitalRead(endstop_X) == LOW) {
//             stableCount++;
//         } else {
//             stableCount = 0; // 如果读到HIGH，重置计数
//         }

//         //delay(10);
//         // 检查Y轴校准是否超时
//         if (millis() - startTime > timeout) {
//             Serial.println("X轴校准超时");
//             return;  // 超时，退出函数
//          }
//     }

//     // X轴限位开关触发，停止X轴电机并设置当前位置为0
//     Xaxis.setCurrentPosition(0);  // 设置X轴当前位置为0
//     //Yaxis.setCurrentPosition(0);  // 设置X轴当前位置为0

//     // Xaxis.moveTo(X_RESET_STEPS);  // 设置X轴目标位置为850步
//     // while (Xaxis.distanceToGo() != 0) {
//     //     Xaxis.run();  // 持续执行直到电机到达目标位置
//     // }
//     // Xaxis.stop();  // 当电机到达目标位置后停止电机
//     Xcalibrated = true;  // 标记X轴已校准
// }
// void calibrateX() {
//
//    while (analogRead(endstop_X) < 3000) {
//        // 向限位开关方向移动直到触发
//        Xaxis.move(10000);  // 每次移动500步
//        Xaxis.run();       // 执行运动
////        Yaxis.move(-10000);  // 每次移动500步
////        Yaxis.run();       // 执行运动
//    }
//
//    // X轴限位开关触发，停止X轴电机并设置当前位置为0
//    Xaxis.setCurrentPosition(0);  // 设置X轴当前位置为0
//    Yaxis.setCurrentPosition(0);  // 设置X轴当前位置为0
//
//    Xaxis.moveTo(X_RESET_STEPS);  // 设置X轴目标位置为850步
//    while (Xaxis.distanceToGo() != 0) {
//        Xaxis.run();  // 持续执行直到电机到达目标位置
//    }
//    Xaxis.stop();  // 当电机到达目标位置后停止电机
//    Xcalibrated = true;  // 标记X轴已校准
//}
// Y轴校准函数
// void calibrateY() {
//     unsigned long startTime = millis(); // 记录开始时间
//     unsigned long timeout = 50000; // 设置超时时间为10秒
//     int stableCount = 0;
//     const int STABILITY_THRESHOLD = 2;

//     while (stableCount < STABILITY_THRESHOLD) {
//         // 向限位开关方向移动直到触发
//         Yaxis.move(10000);  // 每次移动500步
//         Yaxis.run();       // 执行运动
//         Xaxis.move(10000);  // 每次移动500步
//         Xaxis.run();       // 执行运动

//         if (digitalRead(endstop_Y) == LOW) {
//             stableCount++;
//         } else {
//             stableCount = 0; // 如果读到HIGH，重置计数
//         }

//         delay(1);
//         // 检查Y轴校准是否超时
//         if (millis() - startTime > timeout) {
//             Serial.println("Y轴校准超时");
//             return;  // 超时，退出函数
//          }
//     }

//     // Y轴限位开关触发，停止Y轴电机并设置当前位置为0
//     Yaxis.setCurrentPosition(0);  // 设置Y轴当前位置为0
//     // Yaxis.moveTo(Y_RESET_STEPS);  // 设置Y轴目标位置为-4950步
//     // while (Yaxis.distanceToGo() != 0) {
//     //     Yaxis.run();  // 持续执行直到电机到达目标位置
//     // }
//     Yaxis.stop();  // 当电机到达目标位置后停止电机
//     Ycalibrated = true;  // 标记Y轴已校准
// }

//

/**
 * @brief 校准Y轴限位开关
 * @note 包含50秒超时保护机制
 *
 * 校准流程：
 * 1. 持续移动XY两个电机直至触发限位
 * 2. 重置电机位置计数器
 * 3. 返回安全位置并更新校准状态
 */
void calibrateY()
{
    motor_enable = true;
    unsigned long startTime = millis(); // 记录开始时间
    unsigned long timeout = 50000;      // 设置超时时间为10秒
    while (digitalRead(endstop_Y) != LOW)
    {

        // 向限位开关方向移动直到触发
        Yaxis.move(10000); // 每次移动500步
        Yaxis.run();       // 执行运动
        Xaxis.move(10000); // 每次移动500步
        Xaxis.run();       // 执行运动
        // 检查Y轴校准是否超时
        if (millis() - startTime > timeout)
        {
            if (print_key)
            {
                Serial.println("Y轴校准超时");
            }
            return; // 超时，退出函数
        }
    }

    // Y轴限位开关触发，停止Y轴电机并设置当前位置为0
    Yaxis.setCurrentPosition(0); // 设置Y轴当前位置为0
    Yaxis.moveTo(Y_RESET_STEPS); // 设置Y轴目标位置为-4950步
    while (Yaxis.distanceToGo() != 0)
    {
        Yaxis.run(); // 持续执行直到电机到达目标位置
    }
    Yaxis.stop();       // 当电机到达目标位置后停止电机
    Ycalibrated = true; // 标记Y轴已校准
}
bool X_calibrated()
{
    return Xcalibrated;
}

bool Y_calibrated()
{
    return Ycalibrated;
}

/**
 * @brief 完整校准流程入口
 *
 * 执行顺序：
 * 1. 使能电机电源
 * 2. 依次校准X/Y轴
 * 3. 重置所有位置参数
 * 4. 关闭电机电源
 * @warning 校准时需确保机械结构处于安全位置
 */

void calibrate()
{
    MOTOR_ENABLE; // 启动机械臂电机
    motor_enable = true;
    //校准标识
    Xcalibrated = false;
    Ycalibrated = false;
    // Serial.print("last_x_g: ");
    // Serial.print(last_x_g);
    // Serial.print("last_y_g: ");
    // Serial.print(last_y_g);
    // Serial.println();

    if (fabs(last_y_g) < 5.0f)
    {
        if (print_key)
        {
            Serial.println("往左边移动");
        }
        long move_steps = -1000; // 你想移动的步数
        Xaxis.move(move_steps);
        Yaxis.move(move_steps);
        // 持续 run，直到到达目标位置
        while (Xaxis.distanceToGo() != 0 || Yaxis.distanceToGo() != 0)
        {
            Xaxis.run();
            Yaxis.run();
        }
    }

    // 进行X轴校准

    if (!X_calibrated())
    {
        calibrateX(); // 调用X轴校准函数
        if (print_key)
        {
            Serial.println("校准X完毕");
        }
        Xaxis.setCurrentPosition(0); // 设置X轴当前位置为0
        calibration_x_g = true;

        Xaxis.stop();
        Yaxis.stop();
    }

    // 进行Y轴校准

    if (!Y_calibrated())
    {
        if (print_key)
        {
            Serial.println("校准Y");
        }
        calibrateY();
        if (print_key)
        {
            Serial.println("校准y完毕"); // 调用Y轴校准函数
        }
        Yaxis.setCurrentPosition(0); // 设置Y轴当前位置为0
        calibration_y_g = true;
    }

    Xaxis.setCurrentPosition(0); // 设置X轴当前位置为0
    Yaxis.setCurrentPosition(0); // 设置Y轴当前位置为0
    savelocation(0, 0);
    
    // 停止当前运动并重置目标
    Xaxis.stop();
    Yaxis.stop();
    Xaxis.setCurrentPosition(Xaxis.currentPosition());
    Yaxis.setCurrentPosition(Yaxis.currentPosition());
    MOTOR_DISABLE
}
